#include "SteeringFaceBehaviour.h"

SteeringFaceBehaviour::SteeringFaceBehaviour(double ignoreDistance, SteeringAlignBehaviour& alignBehaviour)
    :m_ignoreDistance(fabs(ignoreDistance))
{
    m_alignBehaviour = new SteeringAlignBehaviour(alignBehaviour);
}

SteeringBehaviourOutput* SteeringFaceBehaviour::runFace(Point2D position, Vector2D orientation, double angularSpeed, double maxAngularAcc, Point2D target) {
    Vector2D alignTarget(sub(target, position));
    std::cout<<"Orientation ->"<<orientation.x()<<" ::::::"<<orientation.y()<<std::endl;
    if (alignTarget.length()<this->m_ignoreDistance)
            return this->m_alignBehaviour->runAlign(orientation, angularSpeed, maxAngularAcc, orientation);
    return this->m_alignBehaviour->runAlign(orientation, angularSpeed, maxAngularAcc, alignTarget);
}
